using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection
{
    public struct NonconvexReductionChild
    {
        public ConvexContactManifold Manifold;
        /// <summary>
        /// 从第一个图形的父图形的原点到子图形在世界空间中的位置的偏移。如果没有父级,则这是零向量。
        /// </summary>
        public Vector3 OffsetA;
        public int ChildIndexA;
        /// <summary>
        /// 从第二个图形的父图形的原点到子图形在世界空间中的位置的偏移。如果没有父级,则这是零向量。
        /// </summary>
        public Vector3 OffsetB;
        public int ChildIndexB;
    }

    public struct NonconvexReduction : ICollisionTestContinuation
    {
        public int ChildCount;
        public int CompletedChildCount;
        public Buffer<NonconvexReductionChild> Children;

        public void Create(int childManifoldCount, BufferPool pool)
        {
            ChildCount = childManifoldCount;
            CompletedChildCount = 0;
            pool.Take(childManifoldCount, out Children);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe static void AddContact(ref NonconvexReductionChild child, int contactIndexInChild, NonconvexContactManifold* targetManifold)
        {
            ref var contact = ref Unsafe.Add(ref child.Manifold.Contact0, contactIndexInChild);
            ref var target = ref NonconvexContactManifold.Allocate(targetManifold);
            target.Offset = contact.Offset;
            target.Depth = contact.Depth;
            target.Normal = child.Manifold.Normal;
            // 将凸面生成的特征ID与子索引混合。
            target.FeatureId = contact.FeatureId ^ ((child.ChildIndexA << 8) ^ (child.ChildIndexB << 16));
        }
        struct RemainingCandidate
        {
            public int ChildIndex;
            public int ContactIndex;
            public float Distinctiveness;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe static void UseContact(ref QuickList<RemainingCandidate> remainingCandidates, int index, ref Buffer<NonconvexReductionChild> children, NonconvexContactManifold* targetManifold)
        {
            ref var candidate = ref remainingCandidates[index];
            AddContact(ref children[candidate.ChildIndex], candidate.ContactIndex, targetManifold);
            remainingCandidates.FastRemoveAt(index);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static float ComputeDistinctiveness(in ConvexContact candidate, in Vector3 contactNormal, in NonconvexContact reducedContact, float distanceSquaredInterpolationMin, float inverseDistanceSquaredInterpolationSpan, float depthScale)
        {
            // 一个触点与另一个触点的距离越远,或者它的法线越不同,它就被认为越不同。
            // 目标是清晰度在0到2的范围内。精确值并不是非常重要-我们只需要一个粗略的范围
            // 这样我们才能有意义地融入深度启发式。
            var normalDot = Vector3.Dot(contactNormal, reducedContact.Normal);
            const float normalInterpolationSpan = -0.99999f;
            // 高于阈值的法线网点被认为是完全多余的。Acos(0.99999)约为0.25度。
            // 指向90度角的法线的值为~1,而完全相反的法线的值为~2。
            var normalDistinctiveness = (normalDot - 0.99999f) * (1f / normalInterpolationSpan);

            // 低于阈值时,偏移量被认为是完全冗余的。
            var offsetDistinctiveness = ((reducedContact.Offset - candidate.Offset).LengthSquared() - distanceSquaredInterpolationMin) * inverseDistanceSquaredInterpolationSpan;

            // 我们在法线、偏移量和法线*偏移量显著性上使用三个方向的最大值。正常和偏移分数有时都可以超过1,因此正常*偏移可能高于其他两个。
            // 请注意,位于完全相同位置但法线明显不同的点仍被认为是不同的。
            var combinedDistinctiveness = normalDistinctiveness * offsetDistinctiveness;
            var distinctiveness = offsetDistinctiveness > normalDistinctiveness ? offsetDistinctiveness : normalDistinctiveness;
            if (distinctiveness < combinedDistinctiveness)
                distinctiveness = combinedDistinctiveness;

            if (distinctiveness <= 0)
            {
                distinctiveness = 0;
            }
            else
            {
                // 考虑到接触的深度。更深的接触对歧管更重要。
                var depthMultiplier = 1 + candidate.Depth * depthScale;
                if (depthMultiplier < 0.01f)
                    depthMultiplier = 0.01f;
                distinctiveness *= depthMultiplier;
            }
            return distinctiveness;
        }

        unsafe void ChooseMostDistinct(NonconvexContactManifold* manifold, BufferPool pool)
        {
            // 接触减少的最终目标是选择提供最大程度约束的合理稳定的接触子集。
            // 对于任何给定的评分机制,计算全局最优解都是相当昂贵的,因此我们将进行一些简化：
            // 1)试探性地选择在帧之间合理稳定的起点。将其添加到简化的流形中。
            // 2)在未添加的集合中搜索最大化约束启发式的联系人并添加。将其从未添加的集中删除。
            // 3)重复#2,直到简化的流形中没有空间,或者所有剩余的候选对象与简化的流形都是多余的。

            // 我们首先计算了一些标定数据。知道歧管的大致中心和最大距离可以获得更多的信息阈值。

            var extentAxis = new Vector3(0.280454652f, 0.55873544499f, 0.7804869574f);
            float minimumExtent = float.MaxValue;
            Vector3 minimumExtentPosition = default;
            for (int i = 0; i < ChildCount; ++i)
            {
                ref var child = ref Children[i];
                for (int j = 0; j < child.Manifold.Count; ++j)
                {
                    ref var position = ref Unsafe.Add(ref child.Manifold.Contact0, j).Offset;
                    var extent = Vector3.Dot(position, extentAxis);
                    if (extent < minimumExtent)
                    {
                        minimumExtent = extent;
                        minimumExtentPosition = position;
                    }
                }
            }
            float maximumDistanceSquared = 0;
            for (int i = 0; i < ChildCount; ++i)
            {
                ref var child = ref Children[i];
                for (int j = 0; j < child.Manifold.Count; ++j)
                {
                    var distanceSquared = (Unsafe.Add(ref child.Manifold.Contact0, j).Offset - minimumExtentPosition).LengthSquared();
                    if (distanceSquared > maximumDistanceSquared)
                        maximumDistanceSquared = distanceSquared;
                }
            }
            var maximumDistance = (float)Math.Sqrt(maximumDistanceSquared);
            float initialBestScore = -float.MaxValue;
            int initialBestScoreIndex = 0;
            var maximumAllocatedCandidateCount = ChildCount * 4;
            Buffer<RemainingCandidate> remainingContactsBuffer;
            const int heapAllocationThreshold = 8192;
            if (maximumAllocatedCandidateCount > heapAllocationThreshold)
            {
                pool.Take(maximumAllocatedCandidateCount, out remainingContactsBuffer);
            }
            else
            {
                var memory = stackalloc RemainingCandidate[maximumAllocatedCandidateCount];
                remainingContactsBuffer = new Buffer<RemainingCandidate>(memory, maximumAllocatedCandidateCount);
            }
            var remainingContacts = new QuickList<RemainingCandidate>(remainingContactsBuffer);
            var extremityScale = maximumDistance * 5e-3f;
            for (int childIndex = 0; childIndex < ChildCount; ++childIndex)
            {
                ref var child = ref Children[childIndex];
                ref var childContactsBase = ref child.Manifold.Contact0;
                for (int contactIndex = 0; contactIndex < child.Manifold.Count; ++contactIndex)
                {
                    ref var contact = ref Unsafe.Add(ref childContactsBase, contactIndex);
                    // 请注意,我们只考虑具有积极深度的"极端"联系人,以避免选择纯粹的投机性联系人作为起点。
                    // 如果没有正面深度的接触,那就只靠"最深"的投机性接触就可以了。
                    // 如果没有稳定的接触,特征id稳定性就无关紧要了。
                    float candidateScore;
                    if (contact.Depth >= 0)
                    {
                        // 请注意,我们假设接触偏移量已经以复合对的形式移到父对象的空间中,这样我们就可以有效地比较流形上的范围。
                        var extent = Vector3.Dot(contact.Offset, extentAxis) - minimumExtent;
                        candidateScore = contact.Depth + extent * extremityScale;
                    }
                    else
                    {
                        // 推测性联系得分只是基于深度。
                        candidateScore = contact.Depth;
                    }
                    if (candidateScore > initialBestScore)
                    {
                        initialBestScore = candidateScore;
                        initialBestScoreIndex = remainingContacts.Count;
                    }
                    ref var indices = ref remainingContacts.AllocateUnsafely();
                    indices.ChildIndex = childIndex;
                    indices.ContactIndex = contactIndex;
                    indices.Distinctiveness = float.MaxValue;
                }
            }

            Debug.Assert(remainingContacts.Count > 0, "This function should only be called when there are populated manifolds.");

            UseContact(ref remainingContacts, initialBestScoreIndex, ref Children, manifold);

            var distanceSquaredInterpolationMin = maximumDistanceSquared * (1e-3f * 1e-3f);
            var inverseDistanceSquaredInterpolationSpan = 1f / (maximumDistanceSquared);
            var depthScale = 400f / maximumDistance;

            // 我们现在有了一个不错的起点。现在,递增地搜索尽可能扩展歧管的触点。
            // 这将是一个贪婪和非最优的搜索,但保持一致和足够好比真正的最优更重要。
            var reducedContacts = &manifold->Contact0;
            while (remainingContacts.Count > 0 && manifold->Count < NonconvexContactManifold.MaximumContactCount)
            {
                var lastContactIndex = manifold->Count - 1;
                var reducedContact = reducedContacts + lastContactIndex;

                float bestScore = -1;
                int bestScoreIndex = -1;
                // 注意循环的顺序;我们根据索引选择最佳接触,删除后可以修改索引。
                // 不能(轻易)颠倒顺序。
                for (int i = 0; i < remainingContacts.Count; ++i)
                {
                    ref var remainingContact = ref remainingContacts[i];
                    ref var childManifold = ref Children[remainingContact.ChildIndex].Manifold;
                    ref var childContact = ref Unsafe.Add(ref childManifold.Contact0, remainingContact.ContactIndex);
                    var distinctiveness = ComputeDistinctiveness(childContact, childManifold.Normal, *reducedContact, distanceSquaredInterpolationMin, inverseDistanceSquaredInterpolationSpan, depthScale);
                    if (distinctiveness <= 0)
                    {
                        // 这种联系完全是多余的。
                        remainingContacts.FastRemoveAt(i);
                        --i;
                    }
                    else
                    {
                        // 这次接触并不完全是多余的。更新其唯一性分数;我们在所有测试的联系人中选择唯一性分数最低的联系人。
                        if (distinctiveness < remainingContact.Distinctiveness)
                            remainingContact.Distinctiveness = distinctiveness;
                        if (remainingContact.Distinctiveness > bestScore)
                        {
                            bestScore = remainingContact.Distinctiveness;
                            bestScoreIndex = i;
                        }
                    }
                }
                // 我们可能已经删除了所有剩余的联系人,因此Best ScoreIndex可能仍然是-1。那样的话,我们就完了。
                if (bestScoreIndex >= 0)
                    UseContact(ref remainingContacts, bestScoreIndex, ref Children, manifold);
            }

            if (maximumAllocatedCandidateCount > heapAllocationThreshold)
            {
                remainingContacts.Dispose(pool);
            }
        }


        public unsafe void Flush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
        {
            Debug.Assert(ChildCount > 0);
            if (ChildCount == CompletedChildCount)
            {
                // 此续集已准备就绪,可以进行处理。找到要报告的触点歧管。
                int populatedChildManifolds = 0;
                // 我们缓存一个索引,以防只有一个填充的流形。发现顺序并不重要-只有当存在一个流形时才会使用此值。
                int samplePopulatedChildIndex = 0;
                int totalContactCount = 0;
                for (int i = 0; i < ChildCount; ++i)
                {
                    ref var child = ref Children[i];
                    var childManifoldCount = child.Manifold.Count;
                    if (childManifoldCount > 0)
                    {
                        totalContactCount += childManifoldCount;
                        ++populatedChildManifolds;
                        samplePopulatedChildIndex = i;
                        for (int j = 0; j < child.Manifold.Count; ++j)
                        {
                            // 将所有触点推入父对象的空间。
                            Unsafe.Add(ref child.Manifold.Contact0, j).Offset += child.OffsetA;
                        }
                    }
                }
                var sampleChild = Children.Memory + samplePopulatedChildIndex;

                if (populatedChildManifolds > 1)
                {
                    // 有多个贡献子流形,所以只要假设得到的流形将是非凸的。
                    NonconvexContactManifold reducedManifold;
                    // 我们应该假设支持简化流形的堆栈存储器未初始化。我们依赖于计数,所以手动初始化它。
                    reducedManifold.Count = 0;

                    if (totalContactCount <= NonconvexContactManifold.MaximumContactCount)
                    {
                        // 不需要减量;我们可以安装所有触点。
                        // TODO：如果您有任何多余的联系人删除,则必须在运行此操作之前删除。
                        for (int i = 0; i < ChildCount; ++i)
                        {
                            ref var child = ref Children[i];
                            for (int j = 0; j < child.Manifold.Count; ++j)
                            {
                                AddContact(ref child, j, &reducedManifold);
                            }
                        }
                    }
                    else
                    {
                        ChooseMostDistinct(&reducedManifold, batcher.Pool);
                    }

                    // 流形偏移量B是从形状A原点到形状B原点的偏移。
                    reducedManifold.OffsetB = sampleChild->Manifold.OffsetB - sampleChild->OffsetB + sampleChild->OffsetA;
                    batcher.Callbacks.OnPairCompleted(pairId, ref *&reducedManifold);
                }
                else
                {
                    // 这里有两种可能性：
                    // 1)PopatedChildManifolds==1,而samplePopolatedChildIndex是该唯一填充的流形的索引。我们可以直接上报。
                    // 出于性能原因(凸面约束不需要多条法线并使用更快的摩擦模型),直接报告凸形子流形非常有用。
                    // 2)PopatedChildManifolds==0,samplePopolatedChildIndex为0。假设我们知道仅当预期至少有一个流形时才使用此延续
                    // 而且我们只能在所有流形都为空的情况下才能访问此代码路径,所以报告流形0就完全可以了。
                    // 流形偏移量B是从形状A原点到形状B原点的偏移。
                    sampleChild->Manifold.OffsetB = sampleChild->Manifold.OffsetB - sampleChild->OffsetB + sampleChild->OffsetA;
                    batcher.Callbacks.OnPairCompleted(pairId, ref sampleChild->Manifold);
                }
                batcher.Pool.ReturnUnsafely(Children.Id);
#if DEBUG
                // 这使得检测在释放之后发生的无效访问变得更加容易。
                this = new NonconvexReduction();
#endif
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
            where TCallbacks : struct, ICollisionCallbacks
        {
            Children[report.ChildIndex].Manifold = manifold;
            ++CompletedChildCount;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
        {
            Children[report.ChildIndex].Manifold.Count = 0;
            ++CompletedChildCount;
        }

        public bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
        {
            if (CompletedChildCount == ChildCount)
            {
                Flush(pairId, ref batcher);
                return true;
            }
            return false;
        }
    }
}
